import rclpy
from rclpy.node import Node
import tf2_ros
from geometry_msgs.msg import Twist
from tf2_ros import TransformListener, Buffer
from sensor_msgs.msg import Range
import math




class Control(Node):
    def __init__(self):
        super().__init__('control')
        # 创建 tf2 缓存和监听器
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        
        # 启动定时器周期性的获取 tf 变换
        self.timer = self.create_timer(0.001, self.timer_callback)
        self.cmd_vel_publisher = self.create_publisher(Twist, 'cmd_vel', 10)
        self.height_sub = self.create_subscription(Range, '/height', self.height_callback, 10)
        self.log_timer = self.create_timer(1, self.log_callback)


        self.MAX_LINEAR_SPEED =  10             #最大线速度，单位 cm/s
        self.MAX_ANGULAR_SPEED = 15             #最大角速度,单位 度/s

        self.KP_LINEAR_SPEED_POINT = 0.67        #坐标模式线速度KP
        self.KP_LINEAR_SPEED_DETECT = 0.055      #识别模式线速度KP
        self.KP_ANGULAR_SPEED = 0               #角速度环KP

        self.KP_Z_SPEED = 1
        self.MAX_Z_SPEED = 40

        self.MIN_ERROR_XY_POINT = 5             #坐标模式XY方向允许的最小误差，单位 cm
        self.MIN_ERROR_XY_DETECT = 50         #识别模式XY方向允许的最小误差，单位 像素
        self.MIN_ERROR_YAW = 25                 #YAW上最小角度误差，单位 度、

        ####定义变量####

        #目标变量
        self.targetPointX = 0                   #目标坐标：X，单位 cm
        self.targetPointY = 0                   #目标坐标：Y，单位 cm
        self.targetPointZ = 0                   #目标坐标：Z，单位 cm
        self.targetDetectX = 320                #识别模式X方向目标像素 单位 像素
        self.targetDetectY = 0                #识别模式Y方向目标像素 单位 像素
        self.targetDetectZ = 200                  #识别模式Z方向目标像素 单位 像素
        self.targetYaw = 0                      #目标角度YAW， 单位度
        self.targetNum = 8                      #目标数字
        self.targetHeight = 75                  #目标高度

        #中间变量
        self.distanceXY = 0                     #坐标模式距离目标点距离，单位 cm
        self.errorPointX = 0                    #坐标模式X方向偏差像素值，单位 cm
        self.errorPointY = 0                    #坐标模式Y方向偏差像素值，单位 cm
        self.errorPointZ = 0                    #坐标模式Z方向偏差像素值，单位 cm
        self.errorDetectX = 0                   #识别模式X方向偏差像素值，单位 像素
        self.errorDetectY = 0                   #识别模式Y方向偏差像素值，单位 像素
        self.errorDetectZ = 0                   #识别模式Z方向偏差像素值，单位 像素
        self.actualPositionX = 0                #实际位置x
        self.actualPositionY = 0                #实际位置y  
        self.actualPositionZ = 0                #实际位置z
        self.actualPositionYaw = 0              #实际位置YAW
        self.detectX = 0                        #识别到数字中心X坐标
        self.detectY = 0                        #识别到数字中心Y坐标
        self.detectZ = 0                        #识别到数字中心Z坐标
        self.errorYaw = 0                       #Yaw方向误差值，单位 度
        self.cosTheta = 0                       #目标坐标角度余弦值
        self.sinTheta = 0                       #目标坐标角度正弦值
        self.startTime_Laser = 0                #激光开启时间
        self.presentTime_Laser = 0              #激光当前时间
        self.startTime_Delay = 0                #延时开启时间
        self.presentTime_Delay = 0              #延时当前时间
        self.startTime_Laser = 0                #激光开启时间
        self.presentTime_Laser = 0              #激光当前时间
        self.startTime_Height = 0               #高度变换开启时间
        self.presentTime_Height = 0             #高度变换当前时间
        

        #结果变量
        self.resultLinearSpeedXY = 0            #计算输出XY合速度
        self.resultLinearSpeedX = 0             #计算输出X方向速度
        self.resultLinearSpeedY = 0             #计算输出Y方向速度
        self.resultLinearSpeedZ = 0             #计算输出Z方向速度
        self.resultAngularSpeedYaw = 0          #计算输出Yaw方向角速度
        


        #状态变量
        self.flagMovePoint = 0                  #坐标飞行标志
        self.flagdetect = 0                     #目标点对应的flag
        self.mode = 0                           #飞行模式
        self.detectNum = -1                     #识别到的数字
        self.flagLaser = 0                      #激光灯状态变量，0为关闭， 1为开启
        self.flagDelay = 0                      #延时的状态
        self.flagHeight = 0                     #高度变化的状态
        self.flagland = 0                       #降落状态
        self.flagfly = 0                        #起飞状态

        self.Time_0 = 0
        self.Time = 0
        self.sleep_Time0 = 0
        self.sleep_Time = 25
       
        self.stringMax = ""
        self.stringMin = ""
        self.min_data = -1
        self.max_data = -1

        #绕杆数据
        self.r = -1
        self.angle = None
        self.raogan_state = 0
        #当开始进入识别二维码区域时,若向前飞且在0侧置1， 向后飞且在1侧置2， 向前飞且在1侧置3， 向后飞且在0侧置4， 其余为0
        self.detect_flag = 0
        self.raogan_judge_angle = 30#扫描最小角度
        self.raogan_x = -1
        self.raogan_y = -1
        
        self.raogan_distance = 40

    def tf2_euler_from_quaternion(self, quat):
        
        x, y, z, w = quat
        roll = math.atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))
        pitch = math.asin(2.0 * (w * y - z * x))
        yaw = math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))
        
        return [roll, pitch, yaw]  # 返回欧拉角 (roll, pitch, yaw)

    def pub_cmd_vel(self, x=0, y=0, z=0, yaw=0):
        msg = Twist()
        msg.linear.x = x * 0.01
        msg.linear.y = y * 0.01
        msg.linear.z = z * 0.01
        msg.angular.z = yaw * math.pi / 180

        self.cmd_vel_publisher.publish(msg)

    def calculate_point_speed(self):

        
        #计算距离
        self.distanceXY = math.sqrt(self.errorPointX * self.errorPointX + self.errorPointY * self.errorPointY)

        #计算合速度大小
        self.resultLinearSpeedXY = self.KP_LINEAR_SPEED_POINT * self.distanceXY 
        self.resultLinearSpeedXY = max(min(self.resultLinearSpeedXY, self.MAX_LINEAR_SPEED), -self.MAX_LINEAR_SPEED)

        #计算三角函数
        if self.distanceXY != 0:
            self.cosTheta = self.errorPointX / (self.distanceXY)
            self.sinTheta = self.errorPointY / (self.distanceXY)


        #计算输出速度
        self.resultLinearSpeedX = self.resultLinearSpeedXY * self.cosTheta
        self.resultLinearSpeedY = self.resultLinearSpeedXY * self.sinTheta
        self.resultAngularSpeedYaw = self.KP_ANGULAR_SPEED * self.errorYaw

        self.resultLinearSpeedZ = self.KP_Z_SPEED * self.errorPointZ
        
        #速度限幅
        self.resultAngularSpeedYaw = max(min(self.resultAngularSpeedYaw, self.MAX_ANGULAR_SPEED), -self.MAX_ANGULAR_SPEED)
        self.resultLinearSpeedZ = max(min(self.resultLinearSpeedZ, self.MAX_Z_SPEED), -self.MAX_Z_SPEED)

    #计算识别模式速度
    def calculate_detect_speed(self, ID):
        #计算误差像素
        if (ID == 0):
            self.errorDetectX = -(self.targetDetectX - self.detectX)
        elif (ID == 1):
            self.errorDetectX = (self.targetDetectX - self.detectX)
        #self.errorDetectY = self.targetDetectY - self.detectY
        self.errorDetectZ = (self.targetDetectZ - self.detectZ)
        #计算识别后速度
        self.resultLinearSpeedX = self.errorDetectX * self.KP_LINEAR_SPEED_DETECT
        #self.resultLinearSpeedY = self.errorDetectY * self.KP_LINEAR_SPEED_DETECT
        self.resultLinearSpeedZ = self.errorDetectZ * self.KP_LINEAR_SPEED_DETECT
        #计算限速
        self.resultLinearSpeedX = max(min(self.resultLinearSpeedX, self.MAX_LINEAR_SPEED), -self.MAX_LINEAR_SPEED)
        #self.resultLinearSpeedY = max(min(self.resultLinearSpeedY, self.MAX_LINEAR_SPEED), -self.MAX_LINEAR_SPEED)
        self.resultLinearSpeedZ = max(min(self.resultLinearSpeedZ, self.MAX_LINEAR_SPEED), -self.MAX_LINEAR_SPEED)
    

    #坐标移动,更改目标坐标点
    #返回为真代表已经到达目的地
    def move(self, x, y, z, yaw=0, ignore_z = 0):
        x_act, y_act, yaw_act = self.actualPositionX, self.actualPositionY, self.actualPositionYaw
        z_act = self.actualPositionZ
        self.targetPointX = x
        self.targetPointY = y
        self.targetPointZ = z
        self.targetYaw = yaw
        self.errorPointX = self.targetPointX - x_act   
        self.errorPointY = self.targetPointY - y_act
        self.errorPointZ = self.targetPointZ - z_act
        self.errorYaw = self.targetYaw - yaw_act
        if ignore_z == 0:
            return (abs(self.errorPointX) <= self.MIN_ERROR_XY_POINT and
                    abs(self.errorPointY) <= self.MIN_ERROR_XY_POINT and
                    abs(self.errorYaw) <= self.MIN_ERROR_YAW and
                    abs(self.errorPointZ) <= self.MIN_ERROR_XY_POINT)
        else:
            return (abs(self.errorPointX) <= self.MIN_ERROR_XY_POINT + 6 and
                abs(self.errorPointY) <= self.MIN_ERROR_XY_POINT + 6 and
                abs(self.errorYaw) <= self.MIN_ERROR_YAW and
                abs(self.errorPointZ) <= 10)
        
    
    #判断是否到达目的地
    def judge_point(self):
        return (abs(self.errorPointX) <= self.MIN_ERROR_XY_POINT and
                abs(self.errorPointY) <= self.MIN_ERROR_XY_POINT and
                abs(self.errorYaw) <= self.MIN_ERROR_YAW and
                abs(self.errorPointZ) <= self.MIN_ERROR_XY_POINT)
        
    #判断是否到达识别中心
    def judge_detect(self):
        return (abs(self.errorDetectX) <= self.MIN_ERROR_XY_DETECT)
    
    
    def height_callback(self, msg):
        self.actualPositionZ = msg.range * 100
        
        

    def timer_callback(self):
        try:
            transform = self.tf_buffer.lookup_transform(
                'map',     # 目标坐标系
                'laser',    # 源坐标系
                rclpy.time.Time())  # 当前时间
            
            rotation = transform.transform.rotation
            quat = [rotation.x, rotation.y, rotation.z, rotation.w]
            eular = self.tf2_euler_from_quaternion(quat)
            self.actualPositionYaw = eular[2] * 180 / math.pi
            self.actualPositionX = transform.transform.translation.x * 100
            self.actualPositionY = transform.transform.translation.y * 100
            

 #具体控制写在这#
#############################################___CONTROL_CODE_BEGIN___###############################################################


            if self.flagMovePoint == 0 and self.move(0, 0, 112.5):
                self.MAX_LINEAR_SPEED = 35
                self.flagMovePoint = 1



        

#############################################___CONTROL_CODE_END___################################################################
            if self.mode == 0:
                self.calculate_point_speed()
            elif self.mode == 1:
                self.calculate_detect_speed(self.Detect_Cam)

            self.pub_cmd_vel(self.resultLinearSpeedX, self.resultLinearSpeedY, self.resultLinearSpeedZ, self.resultAngularSpeedYaw)
            #self.pub_cmd_vel(self.actualPositionX, self.actualPositionY ,11, 180)
        except tf2_ros.TransformException as e:
            self.get_logger().warn("No TF")
            #计算速度
    def log_callback(self):
        self.get_logger().info(f"x: {self.actualPositionX:.3f}, y: {self.actualPositionY:.3f}, z:{self.actualPositionZ:.3f}, yaw:{self.actualPositionYaw:.3f}")

        
        





def main(args=None):
    rclpy.init(args=args)
    node = Control()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()
